/*! 
 * Header file for the contact resolution system.
 *
 * \author Devis Rossini
 * \date 19 March 2012
 */
#ifndef PHXCONTACTS_H_INCLUDE_GUARD
#define PHXCONTACTS_H_INCLUDE_GUARD

#include "PhxCore.h"
#include "PhxRigidBody.h"

namespace phx
{
	class ContactResolver;

	/*!
     * A contact represents two bodies in contact. 
	 *
     * \note The contact has no callable functions, it just holds the 
	 * contact details.
     */
    class Contact
	{
		/*!
         * The contact resolver object needs access into the contacts to
         * set and effect the contact.
         */
		friend ContactResolver;

	public:

		/*!
         * Holds the bodies that are involved in the contact. The
         * second of these can be 0 for contacts with the scenery.
         */
        RigidBody* mBodies[2];

		/*!
         * Holds the lateral friction coefficient at the contact.
         */
        real mFriction;

		/*!
         * Holds the normal restitution coefficient at the contact.
         */
        real mRestitution;

        /*!
         * Holds the position of the contact in world coordinates.
         */
        Vector3 mContactPoint;

        /*!
         * Holds the normal of the contact in world coordinates.
         */
        Vector3 mContactNormal;

        /*!
         * Holds the depth of penetration at the contact point. If both
         * bodies are specified then the contact point should be midway
         * between the inter-penetrating points.
         */
        real mPenetration;

        /*!
         * Sets the data that doesn't normally depend on the position
         * of the contact (i.e. the bodies, and their material properties).
         */
        void SetBodyData(RigidBody* one, RigidBody* two, real friction, real restitution);

	protected:

		/*!
         * A transform matrix that converts cooordinates in the contact's
         * frame of reference to world space.
         */
        Matrix3 mContactToWorld;

        /*!
         * Holds the closing velocity at the point of contact.
         */
        Vector3 mContactVelocity;

        /*!
         * Holds the required change in velocity for this contact to be
         * resolved.
         */
        real mDesiredDeltaVelocity;

        /*!
         * Holds the world space position of the contact point relative to
         * centre of each body.
         */
        Vector3 mRelativeContactPosition[2];

    protected:

        /*!
         * Calculates internal data from state data.
         */
        void CalculateInternals(real duration);

        /*!
         * Reverses the contact. This involves swapping the two rigid bodies
         * and reversing the contact normal.
         */
        void SwapBodies();

        /*!
         * Updates the awake state of rigid bodies that are taking
         * place in the given contact. A body will be made awake if it
         * is in contact with a body that is awake.
         */
        void MatchAwakeState();

        /*!
         * Calculates and sets the internal value for the desired delta
         * velocity.
         */
        void CalculateDesiredDeltaVelocity(real duration);

        /*!
         * Calculates and returns the velocity of the contact point on 
		 * the given body.
         */
        Vector3 CalculateLocalVelocity(unsigned bodyIndex, real duration);

        /*!
         * Calculates an orthonormal basis for the contact point, based on
         * the primary friction direction (for anisotropic friction) or
         * a random orientation (for isotropic friction).
         */
        void CalculateContactBasis();

        /*!
         * Applies an impulse to the given body, returning the change in 
		 * velocities.
         */
        //void applyImpulse(const Vector3& impulse, RigidBody* body, Vector3* velocityChange, Vector3* rotationChange);

        /*!
         * Performs an inertia-weighted impulse based resolution of this
         * contact alone.
         */
        void ApplyVelocityChange(Vector3 velocityChange[2], Vector3 rotationChange[2]);

        /*!
         * Performs an inertia weighted penetration resolution of this
         * contact alone.
         */
        void ApplyPositionChange(Vector3 linearChange[2], Vector3 angularChange[2], real penetration);

        /*!
         * Calculates the impulse needed to resolve this contact,
         * given that the contact has no friction.
         */
        Vector3 CalculateFrictionlessImpulse(Matrix3* inverseInertiaTensor);

        /*!
         * Calculates the impulse needed to resolve this contact,
         * given that the contact has a non-zero coefficient of
         * friction.
         */
        Vector3 CalculateFrictionImpulse(Matrix3* inverseInertiaTensor);
    };

	/*!
     * The contact resolution routine. One resolver instance
	 * can be shared for the whole simulation.
	 *
	 * The resolver uses an iterative satisfaction algorithm; it loops
     * through each contact and tries to resolve it. Each contact is
     * resolved locally, which may in turn put other contacts in a worse
     * position. The algorithm then revisits other contacts and repeats
     * the process up to a specified iteration limit. It can be proved
     * that given enough iterations, the simulation will get to the
     * correct result. As with all approaches, numerical stability can
     * cause problems that make a correct resolution impossible.
     *
     * \note In general this resolver is not suitable for stacks of bodies,
     * but is perfect for handling impact, explosive, and flat resting
     * situations.
     */
    class ContactResolver
    {
    protected:

        /*!
         * Holds the number of iterations to perform when resolving
         * velocity.
         */
        unsigned mVelocityIterations;

        /*!
         * Holds the number of iterations to perform when resolving
         * position.
         */
        unsigned mPositionIterations;

        /*!
         * To avoid instability, velocities smaller than this value 
		 * are considered to be zero.
         */
        real mVelocityEpsilon;

        /*!
         * To avoid instability, penetrations smaller than this value are 
		 * considered to be not interpenetrating.
         */
        real mPositionEpsilon;

   public:
   
		/*!
         * Stores the number of velocity iterations used in the
         * last call to resolve contacts.
         */
        unsigned mVelocityIterationsUsed;

        /*!
         * Stores the number of position iterations used in the
         * last call to resolve contacts.
         */
        unsigned mPositionIterationsUsed;

        /*!
         * Creates a new contact resolver with the given number of iterations
         * per resolution call, and optional epsilon values.
         */
        ContactResolver(unsigned iterations, real velocityEpsilon = (real)0.01,
            real positionEpsilon = (real)0.01);

        /*!
         * Creates a new contact resolver with the given number of iterations
         * for each kind of resolution, and optional epsilon values.
         */
        ContactResolver(unsigned velocityIterations, unsigned positionIterations,
            real velocityEpsilon, real positionEpsilon);

        /*!
         * Returns true if the resolver has valid settings and is ready to go.
         */
        bool IsValid()
        {
            return (mVelocityIterations > 0) &&
                   (mPositionIterations > 0) &&
                   (mVelocityEpsilon >= 0.0f) &&
                   (mPositionEpsilon >= 0.0f);
        }

        /*!
         * Sets the number of iterations for each resolution stage.
         */
        void SetIterations(unsigned velocityIterations, unsigned positionIterations);

        /*!
         * Sets the number of iterations for both resolution stages.
         */
        void SetIterations(unsigned iterations);

        /*!
         * Sets the tolerance value for both velocity and position.
         */
        void SetEpsilon(real velocityEpsilon, real positionEpsilon);

        /*!
         * Resolves a set of contacts for both penetration and velocity.
         *
         * \note Contacts that cannot interact with each
         * other should be passed to separate calls to resolveContacts,
         * as the resolution algorithm takes much longer for lots of
         * contacts than it does for the same number of contacts in small
         * sets.
         */
        void ResolveContacts(Contact* contacts, unsigned numContacts, real duration);

    protected:

        /*!
         * Sets up contacts ready for processing. This makes sure their
         * internal data is configured correctly and the correct set of bodies
         * is made alive.
         */
        void PrepareContacts(Contact* contacts, unsigned numContacts, real duration);

        /*!
         * Resolves the velocity issues with the given array of constraints,
         * using the given number of iterations.
         */
        void AdjustVelocities(Contact* contacts, unsigned numContacts, real duration);

        /*!
         * Resolves the positional issues with the given array of constraints,
         * using the given number of iterations.
         */
        void AdjustPositions(Contact* contacts, unsigned numContacts, real duration);

	private:

        /*!
         * Keeps track of whether the internal settings are valid.
         */
        bool mValidSettings;
    };
}

#endif // PHXCONTACTS_H_INCLUDE_GUARD